from math import *
import numpy as np

class C2F:
    def __init__(self,refpoint,egopoint):
        self.refpoint=refpoint
        self.egopoint=egopoint
    def run(self):
        rs=self.refpoint[0]
        rx=self.refpoint[1]
        ry=self.refpoint[2]
        rtheta=self.refpoint[3]
        rkappa=self.refpoint[4]
        rdkappa=self.refpoint[5]
        x=self.egopoint[0]
        y=self.egopoint[1]
        v=self.egopoint[2]
        a=self.egopoint[3]
        theta=self.egopoint[4]
        kappa=self.egopoint[5]
        s_condition = np.zeros(3)
        d_condition = np.zeros(3)

        dx = x - rx
        dy = y - ry

        cos_theta_r = cos(rtheta)
        sin_theta_r = sin(rtheta)

        cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx
        d_condition[0] = copysign(sqrt(dx * dx + dy * dy), cross_rd_nd)

        delta_theta = theta - rtheta
        tan_delta_theta = tan(delta_theta)
        cos_delta_theta = cos(delta_theta)

        one_minus_kappa_r_d = 1 - rkappa * d_condition[0]
        d_condition[1] = one_minus_kappa_r_d * tan_delta_theta

        kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1]

        d_condition[2] = (-kappa_r_d_prime * tan_delta_theta +
                          one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *
                          (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa))

        s_condition[0] = rs
        s_condition[1] = v * cos_delta_theta / one_minus_kappa_r_d

        delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa
        s_condition[2] = ((a * cos_delta_theta -
                           s_condition[1] * s_condition[1] *
                           (d_condition[1] * delta_theta_prime - kappa_r_d_prime)) /
                          one_minus_kappa_r_d)
        self.s_condition=s_condition
        self.d_condition=d_condition
    def get(self):
        return [self.s_condition,self.d_condition]

class F2C:
    def run(self):
        pass